use std::io::Read;
use std::time::Duration;
use imu_test::imu::frame_parser::FrameParser;

fn main() {
    let port_name = "COM5";

    // 创建串口
    let mut port = serialport::new(port_name, 9600)
        .timeout(Duration::from_secs(20))
        .open()
        .unwrap();
    // 串口缓冲区
    let mut read_buf = [0u8; 256];

    // 帧解析器
    let mut parser = FrameParser::new();

    loop {
        let n = port.read(&mut read_buf).unwrap();

        // 逐个字节解析
        // for &byte in &read_buf[..n] {
        //     if let Some(fr) = parser.parse_byte(byte) {
        //         println!(
        //             "{:?}",
        //             fr.iter().map(|&x| format!("{:02x}", x)).collect::<Vec<_>>()
        //         );
        //     }
        // }

        // 从缓冲区里面解析出来一帧数据时发生callback
        parser.parse_buffer(&read_buf[..n], |fr| {
            // callback
            if fr[1] != 0x53 {
                return;
            }
            let (y, p, r) = parse_euler(&fr[2..fr.len() - 1]);

            println!("yaw:{}, pitch: {}, roll: {}", y, p, r);
        });
    }
}


fn parse_euler(raw_data: &[u8]) -> (f64, f64, f64) {
    let roll_l = raw_data[0];
    let roll_h = raw_data[1];
    let pitch_l = raw_data[2];
    let pitch_h = raw_data[3];
    let yaw_l = raw_data[4];
    let yaw_h = raw_data[5];

    // (short) ((short)data_h << 8 | data_l)
    let yaw = (yaw_h as i16) << 8 | (yaw_l as i16);
    let pitch = (pitch_h as i16) << 8 | (pitch_l as i16);
    let roll = (roll_h as i16) << 8 | (roll_l as i16);

    const K: f64 = 180.0 / 32768.0;

    (
        (yaw as f64) * K,
        (pitch as f64) * K,
        (roll as f64) * K
    )
}

